package es.uji.viselab.visualservoing;

import javax.swing.JPanel;
import javax.vecmath.Vector2d;

import es.uji.viselab.experiment.ExperimentParameters;
import es.uji.viselab.image.camera.CameraCatalog;
import es.uji.viselab.image.source.ImageSource;
import es.uji.viselab.math.Vector2;
import es.uji.viselab.model.ModelCreator;

public class SimpleIBVS extends VisualServoingTask {

	public SimpleIBVS(ExperimentParameters ep) {
		super(ep);
		error = 9999999;
	}

	@Override
	public double[] compute(ImageSource image) {

		CameraCatalog camera = ep.getCamParameters();
		double df = camera.getFocalDistanceCorrected();

		Vector2[] targetCoordinateList = getTargetCoordinateList();
		Vector2[] imageCoordinateList = image.getImageCoordinateList();

		// Where is the center of both coordinate-list?
		double[] targetCenter = center(targetCoordinateList[0].x, targetCoordinateList[0].y,
				targetCoordinateList[2].x, targetCoordinateList[2].y);
		double[] imageCenter = center(imageCoordinateList[0].x, imageCoordinateList[0].y,
				imageCoordinateList[2].x, imageCoordinateList[2].y);
		// System.out.println("Target Center: " + targetCenter[0] + ", "+
		// targetCenter[1]);
		// System.out.println("Image Center: " + imageCenter[0] + ", "+
		// imageCenter[1]);

		double distanceX = imageCenter[0] - targetCenter[0];
		double distanceY = imageCenter[1] - targetCenter[1];
		// System.out.println("Distance: " + distanceX + ", "+ distanceY);

		double depthStimation = 0.1; // = 100 cm

		double[] velocities = { 0, 0, 0, 0, 0, 0 };

		velocities[0] = depthStimation * distanceX / df;
		velocities[1] = depthStimation * distanceY / df;

		// Is the target far or nearer ?
		// This is to calculate the translation in the Z coordinate
		double perimeterTarget = 0;

		for (int i = 0; i < 3; i++) {
			perimeterTarget = perimeterTarget
					+ distance(targetCoordinateList[i].x, targetCoordinateList[i].y,
							targetCoordinateList[i + 1].x, targetCoordinateList[i + 1].y);
		}
		perimeterTarget = perimeterTarget
				+ distance(targetCoordinateList[3].x, targetCoordinateList[3].y,
						targetCoordinateList[0].x, targetCoordinateList[0].y);

		double perimeterImage = 0;
		for (int i = 0; i < 3; i++) {
			perimeterImage = perimeterImage
					+ distance(imageCoordinateList[i].x, imageCoordinateList[i].y,
							imageCoordinateList[i + 1].x, imageCoordinateList[i + 1].y);
		}
		perimeterImage = perimeterImage
				+ distance(imageCoordinateList[3].x, imageCoordinateList[3].y,
						imageCoordinateList[0].x, imageCoordinateList[0].y);

		if (perimeterImage > perimeterTarget)
			velocities[2] = -depthStimation * (perimeterImage / perimeterTarget) / 4;
		if (perimeterImage <= perimeterTarget)
			velocities[2] = depthStimation * (perimeterTarget / perimeterImage) / 4;
		velocities[2] = velocities[2] / (Math.abs(distanceX) + Math.abs(distanceY) + 1);
		// System.out.println("Perimeter image: "+ perimeterImage+ ", " +
		// "PerimeterTarget: "+ perimeterTarget);
		double correction = 0;
		if (perimeterImage > perimeterTarget)
			correction = (perimeterImage / perimeterTarget) / 4;
		if (perimeterImage <= perimeterTarget)
			correction = (perimeterTarget / perimeterImage) / 4;
		velocities[0] = velocities[0] * correction;
		velocities[1] = velocities[1] * correction;
		// now time to calculate the rotation among the Z coordinate
		Vector2d v1 = new Vector2d();
		Vector2d v2 = new Vector2d();
		v1.x = targetCoordinateList[0].x - targetCoordinateList[1].x;
		v1.y = targetCoordinateList[0].y - targetCoordinateList[1].y;
		v2.x = imageCoordinateList[0].x - imageCoordinateList[1].x;
		v2.y = imageCoordinateList[0].y - imageCoordinateList[1].y;
		double theta = getAngle(v1, v2);
		theta = theta * 360 / (2 * Math.PI);
		if (theta < 0)
			theta = 360 + theta;
		theta = theta % 90;
		if (theta > 45)
			theta = 90 - theta;
		if (theta > 10)
			velocities[5] = theta * 0.1 * -1;
		else if (theta > 1)
			velocities[5] = theta * 0.5 * -1;
		else
			velocities[5] = 0;

		System.out.println("IBVS Velocities: " + velocities[0] + ", " + velocities[1] + ", "
				+ velocities[2] + ", " + velocities[5]);

		error=distance(targetCenter[0],targetCenter[1],imageCenter[0],imageCenter[1])*4;
		error=error+Math.abs(perimeterImage-perimeterTarget);
		
		notifyNewPositionRequested(this);
		
		return velocities;

	}

	private Vector2[] getTargetCoordinateList() {
		return ModelCreator.get2d(ep.getModel(), ep.getCamParameters());
	}

	private double getAngle(Vector2d v1, Vector2d v2) {
		double angle1 = Math.atan2(v1.y, v1.x);
		double angle2 = Math.atan2(v2.y, v2.x);
		return angle1 - angle2;
	}

	private double distance(double x1, double y1, double x2, double y2) {
		double vectorX = x2 - x1;
		double vectorY = y2 - y1;
		return Math.sqrt(Math.pow(vectorX, 2) + Math.pow(vectorY, 2));
	}

	private double[] center(double x1, double y1, double x2, double y2) {
		double vectorX = x2 - x1;
		double vectorY = y2 - y1;
		double[] xy = new double[2];
		xy[0] = x1 + vectorX / 2;
		xy[1] = y1 + vectorY / 2;
		return xy;
	}

	@Override
	public JPanel getPanel() {
		return null;
	}

	@Override
	public void cameraFeaturePointsChanged(ImageSource i) {
		// compute(i);
	}

	@Override
	public double getError() {
		return error;
	}

}
